#include <Servo.h>    // 声明调用Servo.h库
Servo myservo; // 创建一个舵机对象  车轮舵机
Servo SY; // 创建一个舵机对象  摄像头Y轴
Servo SX; // 创建一个舵机对象  摄像头X轴
//定义引脚
int A=2;  
int B=3;
int C=4;
int D=5;
int GECAOSUDU=6;
int GECAO=7;  
int E=8;//用于控制转向舵机是否启；
int I=11;
int GECAOKAIGUAN=13;
//通信相关参数
String comdata="";
char wifi=0;

// about forward prameter :
    float forward_speed;
    float forward_time;
    float forward_angle;

//小车 yunxingshi de参数（wifi)
    float real_angle;//小车运动前进速度
    float z_distance;//小车运动前进时间
    float x_distance;//小车前进角度
    float y_distance;
    float run_speed;


//小车后退参数（wifi)
    float back_speed;//小车后退运动速度
    float back_time;//小车后退运动时间
    float back_angle;//小车后退运动角度
//设置转向舵机默认角度
//定义割草机参数
float mow_time;
float mow_speed;
float stop_cropper_time;
float stop_car_time;
//小车停止函数
void stop_car(){ //停止
    digitalWrite(A,LOW);
    digitalWrite(B,LOW);
    digitalWrite(C,LOW);
    digitalWrite(D,LOW);
} 
//小车角度变换函数( 保护小车旋转角度 )
void change_angle(float run_angle){
//  digitalWrite(E,HIGH);
   if(run_angle<=66){
   run_angle=66;
   }
  else if(run_angle<=98){
      run_angle==98; 
   }
   else{
       run_angle=run_angle;
   }
   myservo.write(run_angle);
   delay(15);
}
//小车后退函数
void back_mode(float run_speed,float run_time,float run_angle){//后退
    digitalWrite(A,LOW);
    digitalWrite(B,HIGH);
    digitalWrite(C,LOW);
    digitalWrite(D,HIGH);
    change_angle(run_angle);
    analogWrite(I,run_speed);//小车运动速度
    delay(run_time);//小车运动时间毫秒单位
    stop_car();
}
//小车前进函数
void forward_mode(float run_speed,float run_time,float run_angle){//前进
    digitalWrite(A,HIGH); 
    digitalWrite(B,LOW);
    digitalWrite(C,HIGH);
    digitalWrite(D,LOW);
    change_angle(run_angle);
    analogWrite(I,run_speed);//`小车运动速度
    delay(run_time);
    stop_car();
} 
//打开割草机函数
void start_cropper(float mow_speeds){
   analogWrite(GECAOSUDU,mow_speeds);//割草速度
   digitalWrite(GECAOKAIGUAN,HIGH); 
   digitalWrite(GECAO,LOW);
}
//关闭割草机割草工作
void stop_cropper(){
   digitalWrite(GECAOKAIGUAN,LOW);
   digitalWrite(GECAO,LOW);
}
//wifi模式判断函数
void wifi_mode(char wifi_char){
  Serial.print("imput char : ");
  Serial.println(wifi_char);
if ('S' == wifi_char){
      back_mode(30,5000,99);
      Serial.println("小车 正在后退");
      }
      else if ('W' == wifi_char) {
      //Serial.println("FORWARD");
      forward_mode(forward_speed=30,forward_time=2000,forward_angle=88);
      Serial.println("小车 正在前进");
      Serial.print("前进速度：");Serial.println(forward_speed);
      Serial.print("前进时间：");Serial.println(forward_time);
      Serial.print("前进角度：");Serial.println(forward_angle);
      }
      else if ('T' == wifi_char) { 
      // Serial.println("STOP");
      stop_car();
      Serial.println("小车 正在停止");
      }
      else if ('A' == wifi_char) { 
      back_mode(back_speed=30,back_time=2000,back_angle=66);
      Serial.println("小车 正在左转");   
      }     
      else if ('D' == wifi_char) { 
      forward_mode(forward_speed=30,forward_time=2000,forward_angle=98);
      Serial.print("小车 正在右转  right_angle ： ");Serial.println(forward_angle);
      }
}
//置零操作函数
void zeroing(){
String comdata="";
char wifi=0;
//小车前进参数（wifi)
float forward_speed=0;
float forward_time=0;
float forward_angle=0;
//小车后退参数（wifi)
float back_speed=0;
float back_time=0;
float back_angle=0;
}
//舵机恢复默认位置函数
float servo_defualt_angle(Servo my_servo,float now_angle,float goal_angle){
if(now_angle>goal_angle){
 for(int i=now_angle;i>=goal_angle;i--){
   my_servo.write(now_angle);
   delay(15);
 }
}
else if(now_angle<=goal_angle){
 for(int i=now_angle;i<=goal_angle;i++){
   my_servo.write(i);
   delay(15);
 }
}
}

//小车运动至目标点数
float go_to_destination(float angle,float speed,float distance){
angle=88+angle/2; //计算出小车运行的偏离角度
forward_mode(30,1000,angle);
back_mode(30,1000,angle);
//通过速度、距离算小车运行时间
float time=distance/speed;
forward_mode(speed,time,88);
}

//小车在青草存在区域 割草函数
float mow_grass(float angle,float speed,float x_distance,float y_distance){
//保证角度在一定范围内
if(angle>14){
  angle=14;
}else if(angle<-23){
  angle=-22;
}
//打开割草机
start_cropper(100);
float x_time=x_distance/speed;
int mow_number=ceil(y_distance/30);
int mow_status=1;
//如果是右边区域割草函数
if(angle>=0){
angle=90-angle;
forward_mode(30,1000,88+angle/2);
back_mode(30,1000,88-angle/2);
forward_mode(speed,x_distance/speed,88);
//运行至第一个角度，割草机垂直点转弯算法
for(int i=1;i<=mow_number;i++){
int turn=1;
turn=turn%2;
//小车拐角点转弯
if(turn==1){//小车需左转180度
forward_mode(30,2000,70);
back_mode(30,2000,95);
forward_mode(30,1000,88);//这是前进函数
forward_mode(30,2000,70);
back_mode(30,2000,95);
//180度转弯完成
forward_mode(speed,x_distance/speed,88);//运动至第二个点
}
else if(turn==0){//需右转180度
forward_mode(30,2000,95);
back_mode(30,2000,70);
forward_mode(30,1000,88);//这是前进函数
forward_mode(30,2000,95);
back_mode(30,2000,70);
forward_mode(speed,x_distance/speed,88);
}
turn++;
}
return mow_status;
}
// 如果青草在左边区域
else if(angle<0){
angle=90+angle;
forward_mode(30,1000,88+angle/2);
back_mode(30,1000,88-angle/2);
forward_mode(30,2000,95);
back_mode(30,2000,70);
forward_mode(speed,x_distance/speed,88);

//运行至第一个角度，割草机垂直点转弯算法
for(int m=1;m<mow_number;m++){
int Lturn=0;
Lturn=Lturn%2;
if(Lturn==1){//需右转180度
forward_mode(30,2000,95);
back_mode(30,2000,70);
forward_mode(30,1000,88);//这是前进函数
forward_mode(30,2000,95);
back_mode(30,2000,70);
forward_mode(speed,x_distance/speed,88);
}
else if(Lturn==1){//需左转180度
forward_mode(30,2000,70);
back_mode(30,2000,95);
forward_mode(30,1000,88);//这是前进函数
forward_mode(30,2000,70);
back_mode(30,2000,95);
//180度转弯完成
forward_mode(speed,x_distance/speed,88);//运动至第二个点
}
Lturn++;
}
return mow_status;
}
//青草区域割草完毕,返回信号给树莓派
}

//字符串参数处理操作函数
void get_parameter(String pi_info){
String  message= ""; //要分离的字符串
int commaPosition;//存储还没有分离出来的字符串
int i=0;
message=pi_info;
Serial.println(message);//串口打印要分离的字符串
do{
      commaPosition = message.indexOf(',');//检测字符串中的逗号
      if(commaPosition != -1)//如果有逗号存在就向下执行
      {    
        if(i==1){
          Serial.print("the real angle is : "); // now i set forward_speed,forward_angle to go_to_destination()`s and mow_grass()`s prameter ,in winter vacation you can chage them 
          Serial.println(real_angle=message.substring(0,commaPosition).toInt());//打印出第一个逗号位置的字符串
                }
        else if(i==2){
          Serial.print("z_distance is : ");  //z_distance is go to destination distance
          Serial.println(z_distance=message.substring(0,commaPosition).toInt());//打印出第一个逗号位置的字符串
                }
        else if(i==3){
          Serial.print("x_distance is : ");
          Serial.println(x_distance=message.substring(0,commaPosition).toInt());//打印出第一个逗号位置的字符串
               }
        else if(i==4){
          Serial.print("y_distance is  : ");
          Serial.println(y_distance=message.substring(0,commaPosition).toInt());//打印出第一个逗号位置的字符串
               }
          message = message.substring(commaPosition+1, message.length());//打印字符串，从当前位置+1开始
          i++;  
              }
      else
      { //找到最后一个逗号，如果后面还有文字，就打印出来 
         if(message.length() > 0)
         Serial.print("run_speed : ");
         Serial.println(run_speed=message.toInt());  
      }
}
//sstart,30,2000,66,50,4000,98
while(commaPosition >=0);
delay(500);
// the next step is going to run car
go_to_destination(60,30,6000);
//car has get to the destination !!!
mow_grass(60,6000,7000,30);
// mow grass done !!!
}
// 初始设置各参数
void setup () {
 Serial.begin (9600); 
 SX.attach(A2); // 摄像头X轴  X轴舵机居中角度97   推荐角度30-160
 SX.write(90);
 SY.attach(A3); // 将引脚A3上的舵机与声明的舵机对象连接起来 摄像头Y轴
 SY.write(180); //Y轴舵机最大角度180  推荐角度100-180**/
 digitalWrite(GECAO,LOW); 
 myservo.attach(E);  // 将引脚9上的舵机与声明的舵机对象连接起来
 pinMode(A, OUTPUT);
 pinMode(B, OUTPUT);
 pinMode(C, OUTPUT);
 pinMode(D, OUTPUT);
 pinMode(I, OUTPUT);
 pinMode(GECAOSUDU, OUTPUT);
 pinMode(GECAO, OUTPUT);
 pinMode(GECAOKAIGUAN,OUTPUT); 
}
void loop () { 
zeroing();
//wifi 控制脚本
wifi = Serial.read();
//wifi_mode(wifi);
//Serial.print("start wifi mode: ");Serial.println(wifi);
//自动获取、处理 ubuntu 参数

comdata=Serial.readString();
if(comdata.startsWith("start")==true){
get_parameter(comdata); // this function include run and mow grass function;
}
get_parameter(comdata);
//Serial.print("end wifi mode: ");Serial.println(wifi);
delay(1000);
}
